#include "FindEdge.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
///////////////////

template<typename T>
T getMidValueFast(std::vector<T> &valueVec, int p)
{
	size_t count = valueVec.size();
	if (count == 0)
		return (T)0;
	//std::vector< T >::iterator begin = valueVec.begin();
	auto begin = valueVec.begin();
	/*std::vector< T >::iterator*/auto end = valueVec.end();
	/*std::vector< T >::iterator*/ auto p50 = valueVec.begin() + count * p / 100; //p90
	std::nth_element(begin, p50, end);
	return *p50;
}

int  FindEdge::EvaluateImage(unsigned char*&buf, int rawSize, int ImgIndex) //ImgIndex from 1 start
{	
	if (buf == NULL)
		return 0;
	cv::Mat img(rawSize, rawSize, CV_16UC1);
	memcpy(img.data, buf, rawSize*rawSize*sizeof(short));
	cv::Rect cropRect(rawSize / 2 - ISize / 2, rawSize / 2 - ISize / 2, ISize, ISize);
	img = img(cropRect);
		
	for (int i = 0; i < ISize; i++)
	{
		int sum = 0;
		unsigned short *p = img.ptr<unsigned short>(i);
		std::vector<unsigned short> IntsV(p + mStart, p + mEnd);
		bkgVec[i] = getMidValueFast(IntsV, 30);
		//cout << bkgVec[i] << endl; //test			

	}
	//////////////////////////////	
	int p90index = getMidValueFast(bkgVec, 80);
	int p10index = getMidValueFast(bkgVec, 20);
	MeanInts[50 + ImgIndex] = p90index - p10index;
	if (abs(ImgIndex) == 1)
		LintsIndex = p90index - p10index;
	else
	{
		//get detail line Part
		DifInts[50 + ImgIndex] = calcEdgeLine(buf, rawSize, ISize);
	}
	return  p90index - p10index;//to check 
}
//DifInts[50 + ImgIndex] = calcEdgeLine(buf, rawSize, ISize);

int FindEdge::getSubImg(int Ind)
{	
	int p = DifInts[Ind];
	if (p == 0)
		p = 800;
	return p;	
}

int FindEdge::CheckEdge(int Ind) //check MeanIndex and retrun Index
{
	int Hthrold = 400;
	int Htime = 8;
	if (Ind > 4) // 1 2 is low, 3 4 5 6 is High
	{
		int baseInt = MeanInts[51];
		if (baseInt < 30) //second Img should not be an edge
			baseInt = 30;
		int CurInts = MeanInts[50 + Ind];
		if (CurInts>Hthrold && CurInts / baseInt > Htime) //bigger Ints
		{
			HintsIndex = CurInts;
			int countHigh = 0;
			for (int i = 2; i < Ind; i++)
			{
				if (MeanInts[50 + i] / baseInt>Htime)// && MeanInts[i] / MeanInts[i - 1]>2
				{
					countHigh++;
				}
				else
					countHigh = 0;
				if (countHigh == 2) //4 pic to ensure right == 3 + Ind
				{
					//i-3 low, i-2 High	
					if (MeanInts[50 + i - 1] - baseInt > MeanInts[50 + i] - MeanInts[50 + i - 1])
						return 50 + i - 1;
					else
						return 50 + i;

					//break;
				}
			}
		}
	}
	else if (Ind<-4) //second time
	{
		int baseInt = MeanInts[49];
		if (baseInt < 30) //second Img should not be an edge
			baseInt = 30;
		int CurInts = MeanInts[50 + Ind];
		if (CurInts> Hthrold && CurInts / baseInt > Htime) //bigger Ints
		{
			int countHigh = 0;
			for (int i = -2; i > Ind; i--)
			{
				if (MeanInts[50 + i] / baseInt > Htime)// && MeanInts[i] / MeanInts[i - 1]>2
				{
					countHigh++;
				}
				else
					countHigh = 0;
				if (countHigh == 2) //4 pic to ensure right == 3 + Ind
				{
					if (MeanInts[50 + i + 1] - baseInt > MeanInts[50 + i] - MeanInts[50 + i + 1])
						return 50 + i + 1;
					else
						return 50 + i;
				}
			}
		}
	}
	return 0;
}
void FindEdge::Init()
{
	MeanInts.clear();
	MeanInts.resize(100, 0);
	DifInts.clear();
	DifInts.resize(100, 0);
}

int FindEdge::getFirMax(int ind, int base) //abs(ind)>1
{
	if (ind > 0)
	{
		if (MeanInts[50 + ind - 1] / base >4)
		{
			if (MeanInts[50 + ind] - MeanInts[50 + ind - 1] > MeanInts[50 + ind - 1] - MeanInts[50 + ind - 2])
				return ind;
			else
				return ind - 1;
		}
		else
		{
			return ind;
		}
	}
	else
	{
		if (MeanInts[50 + ind + 1] / base >4)
		{
			if (MeanInts[50 + ind] - MeanInts[50 + ind + 1] > MeanInts[50 + ind + 1] - MeanInts[50 + ind + 2])
				return ind;
			else
				return ind + 1;
		}
		else
		{
			return ind;
		}
	}

}
int getBaseM(int *arr, int &base, int Asize = 20)
{
	for (int i = 2; i < 100;) //to test
	{
		int count1 = 0, c1max = 0;
		int count2 = 0;
		for (int c = 0; c < Asize; c++)
		{
			/*if (arr[c] == 0)
				break;*/
			int temp = arr[c] / i;
			//cout << temp << "\t";
			if (temp < 2)
				count1++;
			else
				count1 = 0;
			/*if (temp == 1)
			count2++;*/
			if (c1max<count1)
				c1max = count1;
		}		
		if (c1max > 7)
		{
			base = i / 2;			
			break;
		}
		if (i < 10)
			i = i * 2;
		else
			i += 5;
	}
	return base;
}
int FindEdge::compAll(int Ind) //return first
{
	int HighInts = 6;
	if (LintsIndex > 300)
		HighInts = 2;
	else if (LintsIndex > 200)
		HighInts = 3;
	else if (LintsIndex > 100)
		HighInts = 5;

	int baseMeanInts = 30;
	int maxInd1 = 1, maxInd2 = 1;//maxInd1>maxInd2	
	if (Ind > 0) //18 ImageNumber
	{
		if (MeanInts[51] > 10)
			baseMeanInts = MeanInts[51];
		getBaseM(MeanInts.data() + 51, baseMeanInts);  //find 8pic yes changge

		int maxInd = 1;
		for (int i = 2; i < Ind; i++)
		{
			if (MeanInts[50 + i]>MeanInts[50 + maxInd])
				maxInd = i;
			else if (maxInd != maxInd1 && maxInd != maxInd2)
			{
				if (MeanInts[50 + maxInd] > HighInts*baseMeanInts) //refresh maxInd2 and 1
				{
					if (i - maxInd > 2 || i == Ind - 1)
					{
						if (MeanInts[50 + maxInd] > MeanInts[50 + maxInd2])
						{
							if (MeanInts[50 + maxInd] > MeanInts[50 + maxInd1])
							{
								maxInd2 = maxInd1;
								maxInd1 = maxInd;
							}
							else
								maxInd2 = maxInd;
							maxInd = i;
						}
					}

				}

			}
		}
		/////get real FirMax///////////
		int FirMax = 1;
		if (maxInd1 < 3)
			return 51;
		else
		{
			if (abs(maxInd2) != 1) //two maxInd
			{
				int defMax = abs(maxInd1 - maxInd2);
				if (defMax > 7 && defMax<15 && maxInd2<maxInd1) //yes : two side
					FirMax = getFirMax(maxInd2, baseMeanInts); //Fir side
				else
				{
					FirMax = getFirMax(maxInd1, baseMeanInts); //Fir side
				}
			}
			else
				FirMax = getFirMax(maxInd1, baseMeanInts);
		}
		return FirMax + 50;
	}
	else
	{
		if (MeanInts[49] > 10)
			baseMeanInts = MeanInts[49];
		getBaseM(MeanInts.data() + 30, baseMeanInts);

		int maxInd = -1;
		maxInd1 = -1; maxInd2 = -1;
		for (int i = -2; i > Ind; i--)
		{
			if (MeanInts[50 + i] > MeanInts[50 + maxInd])
				maxInd = i;
			else if (maxInd != maxInd1 && maxInd != maxInd2)
			{
				if (MeanInts[50 + maxInd] > HighInts*baseMeanInts && (maxInd - i > 2 || i == Ind + 1)) //refresh maxInd2 and 1
				{
					if (MeanInts[50 + maxInd] > MeanInts[50 + maxInd2])
					{
						if (MeanInts[50 + maxInd] > MeanInts[50 + maxInd1])
						{
							maxInd2 = maxInd1;
							maxInd1 = maxInd;
						}
						else
							maxInd2 = maxInd;
						maxInd = i;
					}
				}

			}
		}
		/////////
		int FirMax = -1;
		if (maxInd1 > -3) //-1 or -2
			return 49;
		else
		{
			if (abs(maxInd2) != 1) //two maxInd
			{
				int defMax = abs(maxInd1 - maxInd2);
				if (defMax > 7 && defMax<15 && maxInd2>maxInd1) //yes : two side
					FirMax = getFirMax(maxInd2, baseMeanInts); //Fir side
				else
					FirMax = getFirMax(maxInd1, baseMeanInts); //Fir side
			}
			else
				FirMax = getFirMax(maxInd1, baseMeanInts);
		}

		return FirMax + 50;
	}
}

int FindEdge::calcEdgeLine(unsigned char*&buf, int rawSize, int checkSize) //return index, index>0 or <0 means found
{
	if (buf == NULL)
		return 0;
	cv::Mat img(rawSize, rawSize, CV_16UC1);
	memcpy(img.data, buf, rawSize*rawSize*sizeof(short));
	cv::Rect cropRect(rawSize / 2 - checkSize / 2, rawSize / 2 - checkSize / 2, checkSize, checkSize);
	img = img(cropRect);

	float s = 0;
	int b10 = 200, b100 = 20;
	int blockNum = (checkSize - b10) / b100;
	int rPosi = (checkSize - b10) / 2;
	cv::Mat b_m, b_st;
	double maxSM = -1000000;
	double minSM = 1000000;
	int EdgeIndex = -1, lowIndex = -1;
	std::vector<double> mVec(blockNum + 1, 0.0);
	std::vector<double> mDifVec(blockNum + 1, 0.0);
	//std::cout << blockNum << endl;
	for (int i = 0; i < blockNum; i++)
	{
		cv::Rect cutImgSize(rPosi, i*b100, 2 * b10, b10); //from up to down			
		cv::meanStdDev(img(cutImgSize), b_m, b_st);
		double std = b_st.at<double>(0, 0);
		double mean = b_m.at<double>(0, 0);
		double sm = std / mean;
		if (sm > maxSM)
		{
			EdgeIndex = i;
			maxSM = sm;
		}
		else if (sm < minSM)
		{
			minSM = sm;
			lowIndex = i;
		}
		mVec[i] = sm;
		mDifVec[i] = mean;
	}

	int Pindex = 2;

	if (EdgeIndex > 4 && EdgeIndex < blockNum - 5)
	{
		s = mDifVec[EdgeIndex - 5] - mDifVec[EdgeIndex + 5];
		//retSM = maxSM * 2 / (mVec[EdgeIndex - 5] + mVec[EdgeIndex + 5]);	

	}
	else if (EdgeIndex<5) //up 
	{
		s = mDifVec[EdgeIndex] - mDifVec[EdgeIndex + 5];
		//retSM = maxSM / mVec[EdgeIndex + 5];
		//Pindex = 1;
	}
	else  //down
	{
		s = mDifVec[EdgeIndex - 5] - mDifVec[EdgeIndex]; //return ...
		//retSM = maxSM / mVec[EdgeIndex - 5];
		//Pindex = 3;
	}
	if (Pindex == 2 && abs(s)>100)
	{
		int line = EdgeIndex*b100 + b10 / 2;
		int partLine = checkSize * EdgeIndex / blockNum;
		//std::cout << endl << checkSize<<" line " << line << " partLine " << partLine << endl;
		if (s > 0)
			return line;
		else
			return 0 - line;
	}
	else
		return 0;
}

